#include "zf_common_headfile.h"
// #include "Function.h"
// #include "Camera.h"
// #include "System.h"
// #include "DataStore.h"
// #include "Element.h"
// #include "search_line.h"

// cv::Mat colorImage(ROW,COL, CV_8UC3);
// //OpenCV实现画线
// void OpenCV_DrawLine(uint8_t (* ipts)[2],uint8_t * ipts_num,uint8_t blue,uint8_t green,uint8_t red)
// {
//     for(uint8_t i = 0; i < *ipts_num; i++)
//     {
//         cv::circle(colorImage, cv::Point(ipts[i][0], ipts[i][1]), 0.5, cv::Scalar(blue, green,red), -1);  // 半径为3的红色实心圆
//     }
// }
// //OpCV图像转成数组
// void OpenCV_Transfer(uint8_t mt9v03x_image[][MT9V03X_W], const cv::Mat& sourceImage) {  

//     // 将源图像的每个像素复制到目标数组中  
//     for (int i = 0; i < sourceImage.rows; ++i) {  
//         for (int j = 0; j < sourceImage.cols; ++j) {  
//             mt9v03x_image[i][j] = sourceImage.at<uint8_t>(i, j);  
//         }  
//     }  
// }

// /**
//  * 从灰度图转到opencv格式的mat图
//  */
// void OpenCV_Inverse(void)//将处理后的图像经过OpenCV逆转
// {
//     // 将数组数据复制到图像的每个通道
//     for (int i = 0; i < colorImage.rows; ++i)
//     {
//         for (int j = 0; j < colorImage.cols; ++j)
//         {
//             colorImage.at<cv::Vec3b>(i, j) = cv::Vec3b(Gray_Image[i][j],Gray_Image[i][j],Gray_Image[i][j]);
//         }
//     }

// for (int ry = ROW_Start; ry > ROW_Start-iptsr_num; ry--)
// {
//         cv::circle(colorImage,cv::Point(iptsr[ry],ry), 0.5,cv::Scalar(0,0,255), -1);
// }

// for (int ry = ROW_Start; ry > ROW_Start-iptsL_num; ry--)
// {
//         cv::circle(colorImage,cv::Point(iptsL[ry],ry), 0.5,cv::Scalar(255,0,0), -1);
// }

// for (int ry = ROW_Start; ry >= ROW_Start-rpts_num; ry--)
// {
//         cv::circle(colorImage,cv::Point(rpts[ry],ry), 0.5,cv::Scalar(0,255,0), -1);
// }

// // for (int i = 0; i < rpts_num; ++i)
// // {
// //     if(i>=Image_DataStruct.begin_err && i<=(Image_DataStruct.begin_err+11))
// //     {
// //         cv::circle(colorImage, cv::Point(rpts[i],rpts[i][1]), 0.5, cv::Scalar(255,0,0), -1);
// //     }
// //     else
// //     {
// //         cv::circle(colorImage, cv::Point(rpts[i][0],rpts[i][1]), 0.5, cv::Scalar(0,255,0), -1);
// //     }
// // }

// }







//两点计算斜率 适合大斜率判定 是否为直道的判定
float Slope_2Points(uint8_t big,uint8_t small,uint8_t (*ipts))
{
    float dominator,nominator;
    dominator = ipts[big]-ipts[small];//求两点之间x轴差距 以右边线为例 dominator>0
    nominator = big-small;//两点之间y轴之间的差距 
    if(dominator==0){dominator=1;}//防止除飞
    return (nominator/dominator);
}

/**
 * 斜距计算
 * b=y-kx
 */
float Intercept_Calculate(uint8_t y,uint8_t (*ipts),float slope)
{
    return (y-slope*ipts[y]);
}

/**
 * 请注意begin_y要大于end_y
 */
void Patch_Line_Points(uint8_t big_y,uint8_t small_y,float slope,float intercept,uint8_t (*ipts))
{
    uint8_t pi;
    for (pi=big_y;pi>=small_y;pi--)
    {
        ipts[pi] = (uint8_t)((pi-intercept)/slope);
        ipts[pi] = func_limit_ab(ipts[pi],COL_Start,COL_End);

    }

}

/**
 * 
 */
void fit_poly(uint8_t big_y,uint8_t small_y,float a,float b,float c, unsigned char (*ipts))
{
    uint8_t pi;
    for (pi=big_y;pi>=small_y;pi--)
    {
        ipts[pi] = (uint8_t)(a*pi*pi+b*pi+c); //x = ay^2 + by +c
        ipts[pi] = func_limit_ab(ipts[pi],COL_Start,COL_End);

    }

}

//最小二乘法拟合斜率 适合小斜率判定
/*
*@param:起始于begein 终止于end(包括这两点) end>begin
直道的斜率在1~2之间
*@author:C先生
*@date:2024-2-1
*/
float Slope_Calculate(int begin,uint8_t end,uint8_t (*p)[2])
{
if(begin<0)begin=0;//过界保护
float xsum=0,ysum=0,xysum=0,x2sum=0;
int i=0;
for(i=begin;i<=end;i++)
{
   xsum +=(float)p[i][0];
   ysum +=(float)p[i][1];
   xysum+=(float)(p[i][0]*p[i][1]);
   x2sum+=(float)(p[i][0]*p[i][0]);
}

if((end-begin+1)*x2sum-xsum*xsum) //判断除数是否为零
{
return ((end-begin+1)*xysum-xsum*ysum)/((end-begin+1)*x2sum-xsum*xsum);
}
else
{
return 2;
}

}

/**
 * flag mean which load
 */
float Slope_Calculate_Lonest_White(int bigy,uint8_t smally,uint8_t (*p),bool flag)
{

float xsum=0,ysum=0,xysum=0,x2sum=0;
int i=0;
for(i=bigy;i>=smally;i--)
{
   xsum +=(float)p[i];
   ysum +=(float)i;
   xysum+=(float)(i*p[i]);
   x2sum+=(float)(p[i]*p[i]);
}

if((smally-bigy+1)*x2sum-xsum*xsum) //判断除数是否为零
{
return ((smally-bigy+1)*xysum-xsum*ysum)/((smally-bigy+1)*x2sum-xsum*xsum);
}
else
{
    if(flag==0)return 2;
    else return -2;

}

}

//二维数组搬迁函数,把A的数据搬迁到B,包括Start_A到End_A
void copyArrays(uint8_t (*A)[2], uint8_t (*B)[2], uint8_t Start_A, uint8_t End_A,uint8_t Start_B)
{
    uint8_t size_to_copy = (End_A - Start_A + 1) * 2;
    memcpy(B[Start_B], A[Start_A], size_to_copy * sizeof(uint8_t));
}

//三点计算曲率
/*
*@param:参数1:边线数组 参数2:起始点 参数3:三点间隔距离
*@author:C先生
*@date:2024-2-14
*/
float process_fcurvity(uint8_t (* ipts)[2],uint8_t m, uint8_t separate)
{
    float K,q1,AB,BC,AC;
    uint8_t x1 = ipts[m][0];
    uint8_t y1 = ipts[m][1];
    uint8_t x2 = ipts[m+separate][0];
    uint8_t y2 = ipts[m+separate][1];
    uint8_t x3 = ipts[m+2*separate][0];
    uint8_t y3 = ipts[m+2*separate][1];
    int S_of_ABC = ((x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1)) / 2;    //面积的符号表示方向
    q1 = ((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
    AB = sqrt(q1);
    q1 = ((x3 - x2) * (x3 - x2) + (y3 - y2) * (y3 - y2));
    BC = sqrt(q1);
    q1 = ((x3 - x1) * (x3 - x1) + (y3 - y1) * (y3 - y1));
    AC = sqrt(q1);
    if (AB * BC * AC == 0)
    {
        K = 0;
    }
    else
        K = (float)4 * S_of_ABC / (AB * BC * AC);
    return K;
}

/// @brief 整数曲率计算
/// @param ipts 
/// @param start 
/// @param separate 
/// @return 曲率值
int process_icurvity(unsigned char (* ipts)[2],unsigned char start, unsigned char separate)
{
    int K,q1,AB,BC,AC;
    unsigned char x1 = ipts[start][0];
    unsigned char y1 = ipts[start][1];
    unsigned char x2 = ipts[start+separate][0];
    unsigned char y2 = ipts[start+separate][1];
    unsigned char x3 = ipts[start+2*separate][0];
    unsigned char y3 = ipts[start+2*separate][1];
    int S_of_ABC = (((x2 - x1) * (y3 - y1) - (x3 - x1) * (y2 - y1))<<7) / 2;    //面积的符号表示方向
    q1 = ((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1));
    AB = my_sqrt(q1);
    q1 = ((x3 - x2) * (x3 - x2) + (y3 - y2) * (y3 - y2));
    BC = my_sqrt(q1);
    q1 = ((x3 - x1) * (x3 - x1) + (y3 - y1) * (y3 - y1));
    AC = my_sqrt(q1);
    if (AB * BC * AC == 0)
    {
        K = 0;
    }
    else
        K = 4 * S_of_ABC / (AB * BC * AC);
    return K;
}

/// @brief 快速求解平方根
/// @param a 
/// @return 
uint16_t my_sqrt(uint32_t a) //快排     快速排序，对一个32位整数求平方根
{
    uint32_t rem = 0;
    uint32_t root = 0;
    uint32_t divisor = 0;
    int i;
    for(i=0; i<16; i++)
    {
        root <<= 1;
        rem = ((rem << 2) + (a >> 30));
        a <<= 2;
        divisor = (root<<1) + 1;
        if(divisor <= rem)
        {
            rem -= divisor;
            root++;
        }
    }
    return (uint16_t)(root);
}